#ifndef MOTION_CONTROLLER_H_
#define MOTION_CONTROLLER_H_

#ifdef __cplusplus
extern "C"
{
#endif

#include "main.h"
#include "StepperMotor.h"

#define M114_BUFFER_SIZE 256   // 缓冲区方式的大小
#define M114_CACHE_INTERVAL 50 // 缓存方式的时间间隔(ms)

#define DEFAULT_STEP_PULSE_WIDTH 1 // 默认1μs脉冲宽度

// 电机方向设置（1或-1） =====待定
#define A_DIRECTION 1 // A轴电机方向
#define B_DIRECTION 1 // B轴电机方向
#define C_DIRECTION 1 // C轴电机方向
#define D_DIRECTION 1 // D轴电机方向
#define E_DIRECTION 1 // E轴电机方向
#define L_DIRECTION 1 // L轴电机方向
#define M_DIRECTION 1 // M轴电机方向
#define N_DIRECTION 1 // N轴电机方向
#define O_DIRECTION 1 // O轴电机方向
#define P_DIRECTION 1 // P轴电机方向

// 步进电机设置（Steps per unit）
// ==========STEPS_PER_MM = (电机基础步数/圈 × 细分倍数) ÷ 丝杠导程(mm)
#define STEPS_PER_MM_A 3200.00 // A轴每毫米步数 ======== 角度对但时间不对 x16表示微步数？
#define STEPS_PER_MM_B 3200.00 // B轴每毫米步数
#define STEPS_PER_MM_C 3200.00 // C轴每毫米步数
#define STEPS_PER_MM_D 3200.00 // D轴每毫米步数
#define STEPS_PER_MM_E 3200.00 // E轴每毫米步数
#define STEPS_PER_MM_L 3200.00 // L轴每毫米步数
#define STEPS_PER_MM_M 3200.00 // M轴每毫米步数
#define STEPS_PER_MM_N 3200.00 // N轴每毫米步数
#define STEPS_PER_MM_O 3200.00 // O轴每毫米步数
#define STEPS_PER_MM_P 3200.00 // P轴每毫米步数

// #define STEPS_PER_MM_A 200.00 // A轴每毫米步数 ======== 时间对但角度不对
// #define STEPS_PER_MM_B 200.00 // B轴每毫米步数
// #define STEPS_PER_MM_C 200.00 // C轴每毫米步数
// #define STEPS_PER_MM_D 200.00 // D轴每毫米步数
// #define STEPS_PER_MM_E 200.00 // E轴每毫米步数
// #define STEPS_PER_MM_L 200.00 // L轴每毫米步数
// #define STEPS_PER_MM_M 200.00 // M轴每毫米步数
// #define STEPS_PER_MM_N 200.00 // N轴每毫米步数
// #define STEPS_PER_MM_O 200.00 // O轴每毫米步数
// #define STEPS_PER_MM_P 200.00 // P轴每毫米步数

// 运动参数（Max feedrates）
#define MAX_FEEDRATE_A 500.00 // A轴最大进给速度 (mm/s)
#define MAX_FEEDRATE_B 500.00 // B轴最大进给速度 (mm/s)
#define MAX_FEEDRATE_C 500.00 // C轴最大进给速度 (mm/s)
#define MAX_FEEDRATE_D 500.00 // D轴最大进给速度 (mm/s)
#define MAX_FEEDRATE_E 500.00 // E轴最大进给速度 (mm/s)
#define MAX_FEEDRATE_L 500.00 // L轴最大进给速度 (mm/s)
#define MAX_FEEDRATE_M 500.00 // M轴最大进给速度 (mm/s)
#define MAX_FEEDRATE_N 500.00 // N轴最大进给速度 (mm/s)
#define MAX_FEEDRATE_O 500.00 // O轴最大进给速度 (mm/s)
#define MAX_FEEDRATE_P 500.00 // P轴最大进给速度 (mm/s)

// 加速度设置（Max Acceleration）
#define MAX_ACCELERATION_A 530.00 // A轴最大加速度 (mm/s^2)
#define MAX_ACCELERATION_B 530.00 // B轴最大加速度 (mm/s^2)
#define MAX_ACCELERATION_C 530.00 // C轴最大加速度 (mm/s^2)
#define MAX_ACCELERATION_D 530.00 // D轴最大加速度 (mm/s^2)
#define MAX_ACCELERATION_E 530.00 // E轴最大加速度 (mm/s^2)
#define MAX_ACCELERATION_L 530.00 // L轴最大加速度 (mm/s^2)
#define MAX_ACCELERATION_M 530.00 // M轴最大加速度 (mm/s^2)
#define MAX_ACCELERATION_N 530.00 // N轴最大加速度 (mm/s^2)
#define MAX_ACCELERATION_O 530.00 // O轴最大加速度 (mm/s^2)
#define MAX_ACCELERATION_P 530.00 // P轴最大加速度 (mm/s^2)

// 工作区域设置
#define A_MAX_POS 200 // A轴最大行程 (mm)
#define B_MAX_POS 200 // B轴最大行程 (mm)
#define C_MAX_POS 200 // C轴最大行程 (mm)
#define D_MAX_POS 200 // D轴最大行程 (mm)
#define E_MAX_POS 200 // E轴最大行程 (mm)
#define L_MAX_POS 200 // L轴最大行程 (mm)
#define M_MAX_POS 200 // M轴最大行程 (mm)
#define N_MAX_POS 200 // N轴最大行程 (mm)
#define O_MAX_POS 200 // O轴最大行程 (mm)
#define P_MAX_POS 200 // P轴最大行程 (mm)

// 归位偏移量（Home offset）
#define HOME_OFFSET_A 0.00 // A轴归位偏移量 (mm)
#define HOME_OFFSET_B 0.00 // B轴归位偏移量 (mm)
#define HOME_OFFSET_C 0.00 // C轴归位偏移量 (mm)
#define HOME_OFFSET_D 0.00 // D轴归位偏移量 (mm)
#define HOME_OFFSET_E 0.00 // E轴归位偏移量 (mm)
#define HOME_OFFSET_L 0.00 // L轴归位偏移量 (mm)
#define HOME_OFFSET_M 0.00 // M轴归位偏移量 (mm)
#define HOME_OFFSET_N 0.00 // N轴归位偏移量 (mm)
#define HOME_OFFSET_O 0.00 // O轴归位偏移量 (mm)
#define HOME_OFFSET_P 0.00 // P轴归位偏移量 (mm)

    enum HomingState // ===== 没有用到
    {
        NOT_HOMING, // 非归零状态
        HOMING_A,   // 归零A轴
        HOMING_B,   // 归零B轴
        HOMING_C,   // 归零C轴
        HOMING_D,   // 归零D轴
        HOMING_E,   // 归零E轴
        HOMING_L,   // 归零L轴
        HOMING_M,   // 归零M轴
        HOMING_N,   // 归零N轴
        HOMING_O,   // 归零O轴
        HOMING_P,   // 归零P轴
        HOMING_COMPLETE
    };

#ifdef __cplusplus
}
#endif

#ifdef __cplusplus
#include <map>
#include <vector>

class MotionController
{
public:
    MotionController(); // 构造函数：初始化所有成员变量

    void init();                                                                // 初始化系统：串口、引脚、电机等
    void update();                                                              // 主循环更新函数：处理命令、更新电机状态等
    void processCommand(const char *command);                                   // 处理接收到的命令
    void home(const std::map<char, bool> &axisFlags, float homingSpeed = 15.0); // 执行归零操作
    void moveTo(const std::map<char, float> &positions);                        // 移动到指定坐标
    void stop();                                                                // 停止所有运动
    void emergencyStop();                                                       // 紧急停止

    // 获取当前位置
    std::map<char, float> getPositions() const; // 获取所有轴当前位置

    // 获取限位开关状态 =====没用到，checkEndstops()里直接检查了引脚电平
    // std::map<char, bool> getEndstopStatus() const; // 获取所有限位开关状态

    // 速度和加速度控制
    void setFeedrate(float feedrate);         // 设置进给速度
    void setAcceleration(float acceleration); // 设置加速度
    float getCurrentFeedrate() const;         // 获取当前进给速度
    float getCurrentAcceleration() const;     // 获取当前加速度

    // 坐标模式控制
    void setAbsoluteMode(bool absolute); // 设置坐标模式（绝对/相对）
    bool isAbsoluteMode() const;         // 获取当前坐标模式

    // ======== 新增 ========
    void powerOnSelfTest(void);            // 上电自检

    // 新增：限位开关状态缓存（用于按键消抖）
    std::map<char, bool> _lastLimitState;      // 上次稳定状态
    std::map<char, uint32_t> _limitChangeTime; // 状态改变时间
    std::map<char, bool> _debouncedLimitState; // 消抖后的稳定状态
    const uint32_t LIMIT_DEBOUNCE_TIME = 50;   // 消抖时间(毫秒)

private:
    HomingState _homingState = NOT_HOMING;  // 当前归零状态
    std::map<char, bool> _homeRequested;    // 各轴归零请求标志
    unsigned long _homingStartTime = 0;     // 归零开始时间
    const float BACKOFF_DISTANCE = 0.0f;

    // 添加初始化完成标志
    bool _initialized = false;
    uint32_t _initStartTime;
    const uint32_t INIT_DELAY = 2000; // (ms) 2秒初始化延迟

    // 10个电机对象
    StepperMotor _aMotor, _bMotor, _cMotor, _dMotor, _eMotor;
    StepperMotor _lMotor, _mMotor, _nMotor, _oMotor, _pMotor;

    // 电机映射表
    std::map<char, StepperMotor *> motorMap;
    void initMotorMap(); // 新增：初始化电机映射

    // 支持的轴字母集合
    const std::string axisLetters = "ABCDELMNOP";

    // 各电机的当前速度设置 ==== 没用到
    float _aFeedrate; // A轴进给速度
    float _bFeedrate; // B轴进给速度
    float _cFeedrate; // C轴进给速度
    float _dFeedrate; // D轴进给速度
    float _eFeedrate; // E轴进给速度
    float _lFeedrate; // L轴进给速度
    float _mFeedrate; // M轴进给速度
    float _nFeedrate; // N轴进给速度
    float _oFeedrate; // O轴进给速度
    float _pFeedrate; // P轴进给速度
    float _globalFeedrate = 100.0; // 新增：全局进给速度

    bool _absoluteMode = true; // 坐标模式：true = 绝对坐标模式, false = 相对坐标模式

    unsigned long _lastPositionUpdate; // 上次位置更新时间

    void checkEndstops();                 // 检查限位开关状态
    void parseGCode(const char *command); // 解析G代码命令
    void parseMCode(const char *command); // 解析M代码命令
    void reportPosition();                // 报告当前位置
    void reportStatus();                  // 报告系统状态
    void printHelp();                     // 打印帮助信息

    bool _isHoming;             // 归零状态标志
    bool _isEmergencyStop;      // 紧急停止标志
    float _currentAcceleration; // 当前加速度设置

};
#endif // __cplusplus

#endif /* MOTION_CONTROLLER_H_ */